from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    # 声明参数
    declare_method_arg = DeclareLaunchArgument(
        "method",
        default_value="standard",
        description="Method to determine the topic name",
    )
    
    # 声明速度系数参数
    declare_linear_x_scale = DeclareLaunchArgument(
        "linear_x_scale",
        default_value="1.9",
        description="Scaling factor for linear X velocity"
    )
    
    declare_linear_y_scale = DeclareLaunchArgument(
        "linear_y_scale",
        default_value="1.0",
        description="Scaling factor for linear Y velocity"
    )
    
    declare_angular_z_scale = DeclareLaunchArgument(
        "angular_z_scale",
        default_value="1.0",
        description="Scaling factor for angular Z velocity"
    )

    # 获取配置文件路径
    bridge_config_path = os.path.join(
        get_package_share_directory("control"), "config", "bridge_config.yaml"
    )
    
    # 创建桥接节点
    bridge_node = ExecuteProcess(
        cmd=[
            "ros2",
            "run",
            "ros1_bridge",
            "dynamic_bridge",
            "--bridge-config",
            bridge_config_path,
            "--cache-time",
            "1000",
        ],
        output="screen",
    )

    return LaunchDescription(
        [
            declare_method_arg,
            declare_linear_x_scale,
            declare_linear_y_scale,
            declare_angular_z_scale,
            Node(
                package="control",
                executable="control_multimode",
                name="velocity_subscriber_multimode",
                output="screen",
                parameters=[
                    {"mode": "highspeed"},  # 可选: 'standard', 'highspeed', 'terrain'
                    {"linear_x_scale": LaunchConfiguration("linear_x_scale")},
                    {"linear_y_scale": LaunchConfiguration("linear_y_scale")},
                    {"angular_z_scale": LaunchConfiguration("angular_z_scale")},
                ],
            ),
            bridge_node,
        ]
    )